﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

using System.Timers;
using System.Xml;


namespace VehicleTest
{
    public enum BarControlState
    {
        Servo,
        Vehicle
    }

    public partial class FollowerVehicle : Form
    {
        public MainWindows mainwindows_fv;
        private VehicleCommand vehicleCommand_fv;
        private XmlDocument xml_readVehicleParameters = new XmlDocument();

        private int DEL_P;
        private int DEL_N;
        private bool flag_speed;
        private bool flag_servo;
        private byte light_state;

        private System.Timers.Timer Ctest1;
        private System.Timers.Timer Ctest2;
        private System.Timers.Timer Ctest3;
        private System.Timers.Timer Ctest4;
        private System.Timers.Timer show_coordinate;
        private System.Timers.Timer BarControl;

        private int Ctest1_count;
        private int Ctest1_state;
        private int Ctest1_times;

        private BarControlState barControlState = BarControlState.Vehicle;
        private bool barControl_start = false; //为了制止初始化时由于滚条值变化而做出动作
        private int servo_pos_bar = 0;
        private int servo_pos_bar_temp = 0;
        private byte servo_num = 1;  //单个舵机调试时的编号
        private bool time_up = false;
        //private bool bar_servo_change = false;
        //private bool bar_motor_change = false;

        #region 键盘控制变量声明
        private bool flag_keyboardcontrol = false;
        private bool flag_W_on = false;
        private bool flag_S_on = false;
        private bool flag_A_on = false;
        private bool flag_D_on = false;
        private bool flag_servo_hold = false;
        private bool flag_sendcontinue = false;
        private int key_state;

        private bool flag_W_temp = false;
        private bool flag_S_temp = false;
       
        private int speed_state;
        private System.Timers.Timer key_ctrl;

        private bool state_change = false;

        private int speed_up_count;
        private int speed_down_count;
        private bool flag_speed_up_stop = false;
        private bool flag_speed_up_stop_n = false;
        private bool flag_speed_down_stop = false;
        private bool flag_speed_down_stop_n = false;
        private bool flag_speed_change1 = false;
        private bool flag_speed_change2 = false;

        private int speed_key_ctrl;

        private int speed_key;
        private UInt16 servo_key;
        private int servo_del_key;
        private int speed_temp_judge;
        private UInt16 servo_temp_judge;
        #endregion
        
        public FollowerVehicle(MainWindows mw)
        {
            InitializeComponent();
            mainwindows_fv = mw;
            vehicleCommand_fv = new VehicleCommand(mw);

            mainwindows_fv.FollowerVehicle_open = true;
            mainwindows_fv.receive_state = ReceiveState.VehiclePacket;

            flag_servo = false;
            flag_speed = false;

            DEL_P = vehicleCommand_fv.SERVO_MAX - vehicleCommand_fv.SERVO_MID;
            DEL_N = vehicleCommand_fv.SERVO_MIN - vehicleCommand_fv.SERVO_MID;
            textBox_Parameter1.Text = vehicleCommand_fv.SERVO_MIN.ToString();
            textBox_Parameter2.Text = vehicleCommand_fv.SERVO_MAX.ToString();
            textBox_Parameter3.Text = vehicleCommand_fv.SERVO_MID.ToString();
            textBox_Parameter4.Text = vehicleCommand_fv.SPEED_MAX.ToString();
            textBox_Parameter5.Text = vehicleCommand_fv.SPEED_MIN.ToString();
            textBox_Parameter6.Text = vehicleCommand_fv.SERVO_Cali.ToString();

            button_ParameterSure.Enabled = false;
            button_SaveParameters.Enabled = false;
            button_Sure.Enabled = false;
            button_CtestExit.Enabled = false;

            //滚动条的相关初始化
            trackBar_Servo.Minimum = vehicleCommand_fv.SERVO_MIN;
            trackBar_Servo.Maximum = vehicleCommand_fv.SERVO_MAX;
            trackBar_Servo.Value = vehicleCommand_fv.SERVO_MID;
            textBox_ShowServo.Text = trackBar_Servo.Value.ToString();
            trackBar_Motor.Minimum = vehicleCommand_fv.SPEED_MIN;
            trackBar_Motor.Maximum = vehicleCommand_fv.SPEED_MAX;
            trackBar_Motor.Value = 0;
            textBox_ShowMotor.Text = trackBar_Motor.Value.ToString();
            
            button_BarControlReset.Enabled = false;
            trackBar_Servo.Enabled = false;
            trackBar_Motor.Enabled = false;
            numericUpDown_ServoNum.Enabled = false;
            button_ServoBreak.Enabled = false;
            //定时器的相关初始化
            Ctest1 = new System.Timers.Timer();
            Ctest1.Interval = 200;
            Ctest1.Enabled = Enabled;
            Ctest1.Stop();
            Ctest1.Elapsed += new ElapsedEventHandler(Ctest1_tick);

            Ctest2 = new System.Timers.Timer();
            Ctest2.Interval = 200;
            Ctest2.Enabled = Enabled;
            Ctest2.Stop();
            Ctest2.Elapsed += new ElapsedEventHandler(Ctest2_tick);

            Ctest3 = new System.Timers.Timer();
            Ctest3.Interval = 200;
            Ctest3.Enabled = Enabled;
            Ctest3.Stop();
            Ctest3.Elapsed += new ElapsedEventHandler(Ctest3_tick);

            Ctest4 = new System.Timers.Timer();
            Ctest4.Interval = 200;
            Ctest4.Enabled = Enabled;
            Ctest4.Stop();
            Ctest4.Elapsed += new ElapsedEventHandler(Ctest4_tick);

            key_ctrl = new System.Timers.Timer();
            key_ctrl.Interval = 150;
            key_ctrl.Enabled = Enabled;
            key_ctrl.Stop();
            key_ctrl.Elapsed += new ElapsedEventHandler(Key_Control_tick);

            show_coordinate = new System.Timers.Timer();
            show_coordinate.Interval = 500;
            show_coordinate.Enabled = Enabled;
            show_coordinate.Stop();
            show_coordinate.Elapsed += new ElapsedEventHandler(Show_Coordinate_tick);

            BarControl = new System.Timers.Timer();
            BarControl.Interval = 150;
            BarControl.Enabled = Enabled;
            BarControl.Stop();
            BarControl.Elapsed += new ElapsedEventHandler(BarControl_tick);
        }

        #region  自定义函数
        //发送舵机和电机控制信息
        private void Action(string spd_dir, string speed_h,string speed_l, string servo_pos_h, string servo_pos_l)
        {
            int speed = 0;
            int servo_del = 0;
            byte[] data = new byte[255];
            int length =0;

            speed = (((int)Convert.ToByte(speed_h, 16)) << 8) + Convert.ToByte(speed_l, 16);
            if (spd_dir == "01")
            {
                speed = -speed;
            }
            servo_del = (((int)Convert.ToByte(servo_pos_h, 16)) << 8) + Convert.ToByte(servo_pos_l, 16) - vehicleCommand_fv.SERVO_MID;
            length = vehicleCommand_fv.ConvertToPacket_Vehicle(data, 0, speed, servo_del, 0);
            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Purple;
                richTextBox_show.AppendText(show.ToUpper() + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }
        //只发送速度信息
        private void Action_m(string spd_dir, string speed_h, string speed_l)
        {
            int speed = 0;
            byte[] data = new byte[255];
            int length = 0;

            speed = (((int)Convert.ToByte(speed_h, 16)) << 8) + Convert.ToByte(speed_l, 16);
            if (spd_dir == "01")
            {
                speed = -speed;
            }
            length = vehicleCommand_fv.ConvertToPacket_Vehicle(data, 1, speed, 0, 0);
            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Purple;
                richTextBox_show.AppendText(show.ToUpper() + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }
        //只发送舵机控制信息
        private void Action_s(string servo_pos_h, string servo_pos_l)
        {
            int servo_del = 0;
            byte[] data = new byte[255];
            int length = 0;

            servo_del = (((int)Convert.ToByte(servo_pos_h, 16)) << 8) + Convert.ToByte(servo_pos_l, 16) - vehicleCommand_fv.SERVO_MID;
            length = vehicleCommand_fv.ConvertToPacket_Vehicle(data, 2, 0, servo_del, 0);
            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Purple;
                richTextBox_show.AppendText(show.ToUpper() + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }
        //开关红外
        private void Light_Ctrl(byte num, bool on)   //num为红外号，on为true时为开红外，on为false是为关红外
        {
            if (on == true)
            {
                light_state &= (byte)~(0x01 << (num - 1));
            }
            else
            {
                light_state |= (byte)(0x01 << (num - 1));
            }

            byte[] data = new byte[255];
            int length = 0;

            length = vehicleCommand_fv.ConvertToPacket_Vehicle(data, 3, 0, 0, light_state);
            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Purple;
                richTextBox_show.AppendText(show.ToUpper() + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }

        private void Servo_Start(byte num) //舵机开始运行
        {
            byte[] data = new byte[8];
            int length = 8;

            data[0] = 0xFF;
            data[1] = 0xFF;
            data[2] = num; //舵机编号
            data[3] = 0x04; //长度-4
            data[4] = 0x02; //表示写指令
            data[5] = 0x41; //寄存器地址,REG_ACTION
            data[6] = 0x01; //舵机保持运行状态

            byte sum = 0;
            for (int i = 2; i < (length-1); i++)
                sum += data[i];
            data[7] = (byte)~((int)sum);

            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Purple;
                richTextBox_show.AppendText(show.ToUpper() + "\n" + "启动舵机" + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }

        private void Servo_Break(byte num) //舵机刹车
        {
            byte[] data = new byte[8];
            int length = 8;

            data[0] = 0xFF;
            data[1] = 0xFF;
            data[2] = num; //舵机编号
            data[3] = 0x04; //长度-4
            data[4] = 0x02; //表示写指令
            data[5] = 0x3C; //寄存器地址,BREAK
            data[6] = 0x01; //驱动芯片直接不使能,电机内无电流流过,这种刹车使用后,需要重新使能运行,舵机才能运动

            byte sum = 0;
            for (int i = 2; i < (length - 1); i++)
                sum += data[i];
            data[7] = (byte)~((int)sum);

            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Red;
                richTextBox_show.AppendText(show.ToUpper() + "\n" + "舵机刹车" + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }

        private void Servo_Move_T(byte num) //舵机以T曲线运行
        {
            byte[] data = new byte[8];
            int length = 8;

            data[0] = 0xFF;
            data[1] = 0xFF;
            data[2] = num; //舵机编号
            data[3] = 0x04; //长度-4
            data[4] = 0x02; //表示写指令
            data[5] = 0x3D; //寄存器地址,REG_MODEL
            data[6] = 0x01; //运行连续轨迹位置模式

            byte sum = 0;
            for (int i = 2; i < (length - 1); i++)
                sum += data[i];
            data[7] = (byte)~((int)sum);

            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Purple;
                richTextBox_show.AppendText(show.ToUpper() + "\n" + "连续曲线运行" + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }

        private void Servo_AddPoints_T(byte num, byte ph, byte pl) //增加舵机节点
        {
            byte[] data = new byte[16];
            int length = 16;

            data[0] = 0xFF;
            data[1] = 0xFF;
            data[2] = num; //舵机编号
            data[3] = 0x0C; //长度-4
            data[4] = 0x02; //表示写指令
            data[5] = 0x38; //寄存器地址,REG_T_POSITION_HI
            data[6] = ph;
            data[7] = 0x39; //寄存器地址,REG_T_POSITION_LO
            data[8] = pl;
            data[9] = 0x3A; //寄存器地址,REG_T_VELOCITY_HI
            data[10] = 0x00;
            data[11] = 0x3B;//寄存器地址,REG_T_VELOCITY_LO
            data[12] = 0x60;
            data[13] = 0x3F;//寄存器地址,REG_CURVE_APPEND
            data[14] = 0x02;//连续轨迹模式增加节点

            byte sum = 0;
            for (int i = 2; i < (length - 1); i++)
                sum += data[i];
            data[15] = (byte)~((int)sum);

            mainwindows_fv.SerialSend2(data, length);

            //在子窗口中显示发送指令
            string show = null;
            show = mainwindows_fv.ByteArrayToHexString_length(data, length);

            richTextBox_show.Invoke(new EventHandler(delegate
            {
                richTextBox_show.SelectionColor = Color.Purple;
                richTextBox_show.AppendText(show.ToUpper() + "\n");
                richTextBox_show.ScrollToCaret();
            }));
        }

        private void Delay(int ms)
        {
            DateTime start = System.DateTime.Now;
            DateTime end;
            TimeSpan time;
            int delay = 0;
            while (delay < ms)
            {
                end = System.DateTime.Now;
                time = end - start;
                delay = time.Milliseconds;
            }
        }

        public void PreClosing()
        {
            mainwindows_fv.FollowerVehicle_open = false;
            mainwindows_fv.receive_state = ReceiveState.VehiclePacket;
            AllTime_Tick_Stop();
        }

        protected override void OnFormClosing(FormClosingEventArgs e)
        {
            PreClosing();
            this.Hide();
            this.Dispose();
        }

        private void AllTime_Tick_Stop()
        {
            Ctest1.Stop();
            Ctest2.Stop();
            Ctest3.Stop();
            Ctest4.Stop();
            key_ctrl.Stop();
            show_coordinate.Stop();
            BarControl.Stop();
        }
        #endregion

        #region 控件事件
        private void button_Sure_Click(object sender, EventArgs e)
        {
            string dir_str = null;
            string spd_h_str = null;
            string spd_l_str = null;
            string svo_h_str = null;
            string svo_l_str = null;
            byte spd_h = 0;
            byte spd_l = 0;
            byte svo_h = 0;
            byte svo_l = 0;

            int speed = 0;
            UInt16 servo_pos = 0;

            speed = int.Parse(textBox_MotoSpeed.Text);
            servo_pos = (UInt16)(vehicleCommand_fv.SERVO_MID + int.Parse(textBox_ServoPos.Text));   //转换舵机参数

            if (speed >= 0)
            {
                dir_str = "00";
            }
            else
            {
                dir_str = "01";
                speed = -speed;
            }

            spd_h = (byte)(speed >> 8);
            spd_l = (byte)(speed & 0x0FF);
            svo_h = (byte)(servo_pos >> 8);
            svo_l = (byte)(servo_pos & 0x0FF);

            if (spd_h >= 16)
            {
                spd_h_str = Convert.ToString(spd_h, 16);
            }
            else
            {
                spd_h_str = "0" + Convert.ToString(spd_h, 16);
            }
            if (spd_l >= 16)
            {
                spd_l_str = Convert.ToString(spd_l, 16);
            }
            else
            {
                spd_l_str = "0" + Convert.ToString(spd_l, 16);
            }
            if (svo_h >= 16)
            {
                svo_h_str = Convert.ToString(svo_h, 16);
            }
            else
            {
                svo_h_str = "0" + Convert.ToString(svo_h, 16);
            }
            if (svo_l >= 16)
            {
                svo_l_str = Convert.ToString(svo_l, 16);
            }
            else
            {
                svo_l_str = "0" + Convert.ToString(svo_l, 16);
            }

            Action(dir_str, spd_h_str, spd_l_str, svo_h_str, svo_l_str);
        }

        private void textBox_MotoSpeed_TextChanged(object sender, EventArgs e)
        {
            int speed = 0;

            try
            {
                speed = int.Parse(textBox_MotoSpeed.Text);
                if ((speed >= vehicleCommand_fv.SPEED_MIN) && (speed <= vehicleCommand_fv.SPEED_MAX))
                {
                    flag_speed = true;
                }
                else
                {
                    flag_speed = false;
                }

                if ((flag_servo == true) && (flag_speed == true))
                {
                    button_Sure.Enabled = true;
                }
                else
                {
                    button_Sure.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_Sure.Enabled = false;
            }
           
        }

        private void textBox_ServoPos_TextChanged(object sender, EventArgs e)
        {
            int del = 0;

            try
            {
                del = int.Parse(textBox_ServoPos.Text);
                if ((del >= DEL_N) && (del <= DEL_P))
                {
                    flag_servo = true;
                }
                else
                {
                    flag_servo = false;
                }

                if ((flag_servo == true) && (flag_speed == true))
                {
                    button_Sure.Enabled = true;
                }
                else
                {
                    button_Sure.Enabled = false;
                }
            }
            catch(FormatException)
            {
                button_Sure.Enabled = false;
            }
        }

        private void button_clear_Click(object sender, EventArgs e)
        {
            richTextBox_show.Clear();
        }

        private void textBox_Parameter1_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int temp1 = Convert.ToInt32(textBox_Parameter1.Text);
                int temp2 = Convert.ToInt32(textBox_Parameter2.Text);
                int temp3 = Convert.ToInt32(textBox_Parameter3.Text);
                int temp4 = Convert.ToInt32(textBox_Parameter4.Text);
                int temp5 = Convert.ToInt32(textBox_Parameter5.Text);
                int temp6 = Convert.ToInt32(textBox_Parameter6.Text);

                if (temp1 >= 0 && temp1 < temp3 && temp3 < temp2 && temp2 <= 1023)
                {
                    button_ParameterSure.Enabled = true;
                    button_SaveParameters.Enabled = true;
                }
                else
                {
                    button_ParameterSure.Enabled = false;
                    button_SaveParameters.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_ParameterSure.Enabled = false;
                button_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Parameter2_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int temp1 = Convert.ToInt32(textBox_Parameter1.Text);
                int temp2 = Convert.ToInt32(textBox_Parameter2.Text);
                int temp3 = Convert.ToInt32(textBox_Parameter3.Text);
                int temp4 = Convert.ToInt32(textBox_Parameter4.Text);
                int temp5 = Convert.ToInt32(textBox_Parameter5.Text);
                int temp6 = Convert.ToInt32(textBox_Parameter6.Text);

                if (temp1 >= 0 && temp1 < temp3 && temp3 < temp2 && temp2 <= 1023)
                {
                    button_ParameterSure.Enabled = true;
                    button_SaveParameters.Enabled = true;
                }
                else
                {
                    button_ParameterSure.Enabled = false;
                    button_SaveParameters.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_ParameterSure.Enabled = false;
                button_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Parameter3_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int temp1 = Convert.ToInt32(textBox_Parameter1.Text);
                int temp2 = Convert.ToInt32(textBox_Parameter2.Text);
                int temp3 = Convert.ToInt32(textBox_Parameter3.Text);
                int temp4 = Convert.ToInt32(textBox_Parameter4.Text);
                int temp5 = Convert.ToInt32(textBox_Parameter5.Text);
                int temp6 = Convert.ToInt32(textBox_Parameter6.Text);

                if (temp1 >= 0 && temp1 < temp3 && temp3 < temp2 && temp2 <= 1023)
                {
                    button_ParameterSure.Enabled = true;
                    button_SaveParameters.Enabled = true;
                }
                else
                {
                    button_ParameterSure.Enabled = false;
                    button_SaveParameters.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_ParameterSure.Enabled = false;
                button_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Parameter4_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int temp1 = Convert.ToInt32(textBox_Parameter1.Text);
                int temp2 = Convert.ToInt32(textBox_Parameter2.Text);
                int temp3 = Convert.ToInt32(textBox_Parameter3.Text);
                int temp4 = Convert.ToInt32(textBox_Parameter4.Text);
                int temp5 = Convert.ToInt32(textBox_Parameter5.Text);
                int temp6 = Convert.ToInt32(textBox_Parameter6.Text);

                if (temp4 > 0)
                {
                    button_ParameterSure.Enabled = true;
                    button_SaveParameters.Enabled = true;
                }
                else
                {
                    button_ParameterSure.Enabled = false;
                    button_SaveParameters.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_ParameterSure.Enabled = false;
                button_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Parameter5_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int temp1 = Convert.ToInt32(textBox_Parameter1.Text);
                int temp2 = Convert.ToInt32(textBox_Parameter2.Text);
                int temp3 = Convert.ToInt32(textBox_Parameter3.Text);
                int temp4 = Convert.ToInt32(textBox_Parameter4.Text);
                int temp5 = Convert.ToInt32(textBox_Parameter5.Text);
                int temp6 = Convert.ToInt32(textBox_Parameter6.Text);

                if (temp5 < 0)
                {
                    button_ParameterSure.Enabled = true;
                    button_SaveParameters.Enabled = true;
                }
                else
                {
                    button_ParameterSure.Enabled = false;
                    button_SaveParameters.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_ParameterSure.Enabled = false;
                button_SaveParameters.Enabled = false;
            }
        }

        private void textBox_Parameter6_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int temp1 = Convert.ToInt32(textBox_Parameter1.Text);
                int temp2 = Convert.ToInt32(textBox_Parameter2.Text);
                int temp3 = Convert.ToInt32(textBox_Parameter3.Text);
                int temp4 = Convert.ToInt32(textBox_Parameter4.Text);
                int temp5 = Convert.ToInt32(textBox_Parameter5.Text);
                int temp6 = Convert.ToInt32(textBox_Parameter6.Text);

                if (temp5 < 0)
                {
                    button_ParameterSure.Enabled = true;
                    button_SaveParameters.Enabled = true;
                }
                else
                {
                    button_ParameterSure.Enabled = false;
                    button_SaveParameters.Enabled = false;
                }
            }
            catch (FormatException)
            {
                button_ParameterSure.Enabled = false;
                button_SaveParameters.Enabled = false;
            }
        }

        private void button_ParameterSure_Click(object sender, EventArgs e)
        {
            vehicleCommand_fv.SERVO_MIN = int.Parse(textBox_Parameter1.Text);
            vehicleCommand_fv.SERVO_MAX = int.Parse(textBox_Parameter2.Text);
            vehicleCommand_fv.SERVO_MID = int.Parse(textBox_Parameter3.Text);
            vehicleCommand_fv.SPEED_MAX = int.Parse(textBox_Parameter4.Text);
            vehicleCommand_fv.SPEED_MIN = int.Parse(textBox_Parameter5.Text);
            vehicleCommand_fv.SERVO_Cali = int.Parse(textBox_Parameter6.Text);

            DEL_P = vehicleCommand_fv.SERVO_MAX - vehicleCommand_fv.SERVO_MID;
            DEL_N = vehicleCommand_fv.SERVO_MIN - vehicleCommand_fv.SERVO_MID;
            button_ParameterSure.Enabled = false;
        }

        private void button_SaveParameters_Click(object sender, EventArgs e)
        {
            xml_readVehicleParameters.Load(mainwindows_fv.path + "/doc/ParametersModel.xml");
            XmlElement xml_root = xml_readVehicleParameters.DocumentElement;
            //增加节点
            XmlElement newServoMin = xml_readVehicleParameters.CreateElement("ServoMin");
            XmlElement newServoMax = xml_readVehicleParameters.CreateElement("ServoMax");
            XmlElement newServoMid = xml_readVehicleParameters.CreateElement("ServoMid");
            XmlElement newServoCali = xml_readVehicleParameters.CreateElement("ServoCalibration");
            XmlElement newMotorMax = xml_readVehicleParameters.CreateElement("MotorMax");
            XmlElement newMotorMin = xml_readVehicleParameters.CreateElement("MotorMin");
            //元素值
            string servo_min_data = textBox_Parameter1.Text;
            string servo_max_data = textBox_Parameter2.Text;
            string servo_mid_data = textBox_Parameter3.Text;
            string servo_cali_data = textBox_Parameter6.Text;
            string motor_max_data = textBox_Parameter4.Text;
            string motor_min_data = textBox_Parameter5.Text;
            XmlText servoMinText = xml_readVehicleParameters.CreateTextNode(servo_min_data);
            XmlText servoMaxText = xml_readVehicleParameters.CreateTextNode(servo_max_data);
            XmlText servoMidText = xml_readVehicleParameters.CreateTextNode(servo_mid_data);
            XmlText servoCaliText = xml_readVehicleParameters.CreateTextNode(servo_cali_data);
            XmlText motorMaxText = xml_readVehicleParameters.CreateTextNode(motor_max_data);
            XmlText motorMinText = xml_readVehicleParameters.CreateTextNode(motor_min_data);

            //添加元素
            xml_root.AppendChild(newServoMin);
            xml_root.AppendChild(newServoMax);
            xml_root.AppendChild(newServoMid);
            xml_root.AppendChild(newServoCali);
            xml_root.AppendChild(newMotorMax);
            xml_root.AppendChild(newMotorMin);
            newServoMin.AppendChild(servoMinText);
            newServoMax.AppendChild(servoMaxText);
            newServoMid.AppendChild(servoMidText);
            newServoCali.AppendChild(servoCaliText);
            newMotorMax.AppendChild(motorMaxText);
            newMotorMin.AppendChild(motorMinText);

            xml_readVehicleParameters.Save(mainwindows_fv.path + "/doc/VehicleParameters.xml");
            MessageBox.Show("成功存储舵机和电机参数!");
        }

        private void button_Ctest1_Click(object sender, EventArgs e)
        {
            Ctest1.Start();
            Ctest2.Stop();
            Ctest3.Stop();
            Ctest4.Stop();

            button_Ctest1.Enabled = false;
            button_Ctest2.Enabled = false;
            button_Ctest3.Enabled = false;
            button_Ctest4.Enabled = false;
            button_KeyboardControl.Enabled = false;
            button_stopKeyCtrl.Enabled = false;
            //button_clear_key.Enabled = false;
            button_CtestExit.Enabled = true;

            
            Ctest1_times = 0;
            if (Ctest1_state == 100)
            {
                Ctest1_state = 0;
            }
        }

        private void button_Ctest2_Click(object sender, EventArgs e)
        {
            Ctest2.Start();
            Ctest1.Stop();
            Ctest3.Stop();
            Ctest4.Stop();

            button_Ctest1.Enabled = false;
            button_Ctest2.Enabled = false;
            button_Ctest3.Enabled = false;
            button_Ctest4.Enabled = false;
            button_KeyboardControl.Enabled = false;
            button_stopKeyCtrl.Enabled = false;
            //button_clear_key.Enabled = false;
            button_CtestExit.Enabled = true;


        }

        private void button_Ctest3_Click(object sender, EventArgs e)
        {
            Ctest3.Start();
            Ctest1.Stop();
            Ctest2.Stop();
            Ctest4.Stop();

            button_Ctest1.Enabled = false;
            button_Ctest2.Enabled = false;
            button_Ctest3.Enabled = false;
            button_Ctest4.Enabled = false;
            button_KeyboardControl.Enabled = false;
            button_stopKeyCtrl.Enabled = false;
            //button_clear_key.Enabled = false;
            button_CtestExit.Enabled = true;
        }

        private void button_Ctest4_Click(object sender, EventArgs e)
        {
            Ctest4.Start();
            Ctest1.Stop();
            Ctest2.Stop();
            Ctest3.Stop();

            button_Ctest1.Enabled = false;
            button_Ctest2.Enabled = false;
            button_Ctest3.Enabled = false;
            button_Ctest4.Enabled = false;
            button_KeyboardControl.Enabled = false;
            button_stopKeyCtrl.Enabled = false;
            //button_clear_key.Enabled = false;
            button_CtestExit.Enabled = true;
        }

        private void button_CtestExit_Click(object sender, EventArgs e)
        {
            Ctest1.Stop();
            Ctest2.Stop();
            Ctest3.Stop();
            Ctest4.Stop();

            Action_m("00", "00", "00");

            button_Ctest1.Enabled = true;
            button_Ctest2.Enabled = true;
            button_Ctest3.Enabled = true;
            button_Ctest4.Enabled = true;
            button_KeyboardControl.Enabled = true;
            button_stopKeyCtrl.Enabled = true;
            //button_clear_key.Enabled = true;
            button_CtestExit.Enabled = false;
        }

        private void button_KeyboardControl_Click(object sender, EventArgs e)
        {
            flag_keyboardcontrol = true;

            //button_clear.Enabled = false;
            button_Ctest1.Enabled = false;
            button_Ctest2.Enabled = false;
            button_Ctest3.Enabled = false;
            button_Ctest4.Enabled = false;
            button_Sure.Enabled = false;
            //button_clear_key.Enabled = false;

            key_ctrl.Start();
        }

        private void button_stopKeyCtrl_Click(object sender, EventArgs e)
        {
            flag_keyboardcontrol = false;

            //button_clear.Enabled = true;
            button_Ctest1.Enabled = true;
            button_Ctest2.Enabled = true;
            button_Ctest3.Enabled = true;
            button_Ctest4.Enabled = true;
            //button_clear_key.Enabled = true;

            key_ctrl.Stop();
        }

        private void checkBox_KeyContinueControl_CheckedChanged(object sender, EventArgs e)
        {
            if (checkBox_KeyContinueControl.CheckState == CheckState.Checked)
            {
                flag_sendcontinue = true;
            }
            else
            {
                flag_sendcontinue = false;
            }
        }

        private void button_KeyboardControl_KeyDown(object sender, KeyEventArgs e)
        {
            if (flag_keyboardcontrol == true)
            {
                if (e.KeyCode == Keys.W)
                {
                    flag_W_on = true;
                }
                else if (e.KeyCode == Keys.A)
                {
                    flag_A_on = true;
                }
                else if (e.KeyCode == Keys.D)
                {
                    flag_D_on = true;
                }
                else if (e.KeyCode == Keys.S)
                {
                    flag_S_on = true;
                }
                else if (e.KeyCode == Keys.ShiftKey)
                {
                    flag_servo_hold = true;
                }

                if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 0;  //初始状态
                }
                else if ((flag_W_on == true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 1;  //直线前进
                }
                else if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on == true) && (flag_D_on != true))
                {
                    key_state = 2;  //直线后退
                }
                else if ((flag_W_on != true) && (flag_A_on == true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 3;  //左拐滑行
                }
                else if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on == true))
                {
                    key_state = 4;  //右拐滑行
                }
                else if ((flag_W_on == true) && (flag_A_on == true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 5;  //左拐前进
                }
                else if ((flag_W_on == true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on == true))
                {
                    key_state = 6;  //右拐前进
                }
                else if ((flag_W_on != true) && (flag_A_on == true) && (flag_S_on == true) && (flag_D_on != true))
                {
                    key_state = 7;  //左拐后退
                }
                else if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on == true) && (flag_D_on == true))
                {
                    key_state = 8;  //右拐后退
                }


                //速度控制
                if ((flag_W_temp == false) && (flag_W_on == true))
                {
                    speed_state = 1; //加速前进
                }
                else if ((flag_S_temp == false) && (flag_S_on == true))
                {
                    speed_state = 2; //加速后退
                }

                if (flag_W_on == true)
                {
                    flag_W_temp = true;
                }
                if (flag_S_on == true)
                {
                    flag_S_temp = true;
                }
            }
        }

        private void button_KeyboardControl_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (flag_keyboardcontrol == true)
            {

            }
        }

        private void button_KeyboardControl_KeyUp(object sender, KeyEventArgs e)
        {
            if (flag_keyboardcontrol == true)
            {
                if (e.KeyCode == Keys.W)
                {
                    flag_W_on = false;
                }
                if (e.KeyCode == Keys.A)
                {
                    flag_A_on = false;
                }
                if (e.KeyCode == Keys.D)
                {
                    flag_D_on = false;
                }
                if (e.KeyCode == Keys.S)
                {
                    flag_S_on = false;
                }
                if (e.KeyCode == Keys.ShiftKey)
                {
                    flag_servo_hold = false;
                }

                if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 0;  //初始状态
                }
                else if ((flag_W_on == true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 1;  //直线前进
                }
                else if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on == true) && (flag_D_on != true))
                {
                    key_state = 2;  //直线后退
                }
                else if ((flag_W_on != true) && (flag_A_on == true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 3;  //左拐滑行
                }
                else if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on == true))
                {
                    key_state = 4;  //右拐滑行
                }
                else if ((flag_W_on == true) && (flag_A_on == true) && (flag_S_on != true) && (flag_D_on != true))
                {
                    key_state = 5;  //左拐前进
                }
                else if ((flag_W_on == true) && (flag_A_on != true) && (flag_S_on != true) && (flag_D_on == true))
                {
                    key_state = 6;  //右拐前进
                }
                else if ((flag_W_on != true) && (flag_A_on == true) && (flag_S_on == true) && (flag_D_on != true))
                {
                    key_state = 7;  //左拐后退
                }
                else if ((flag_W_on != true) && (flag_A_on != true) && (flag_S_on == true) && (flag_D_on == true))
                {
                    key_state = 8;  //右拐后退
                }

                //速度控制
                if ((flag_W_temp == true) && (flag_W_on == false))
                {
                    speed_state = 3; //减速前进
                }
                else if ((flag_S_temp == true) && (flag_S_on == false))
                {
                    speed_state = 4; //减速后退
                }

                if (flag_W_on == false)
                {
                    flag_W_temp = false;
                }
                if (flag_S_on == false)
                {
                    flag_S_temp = false;
                }
            }
        }

        private void button_clear_key_Click(object sender, EventArgs e)
        {
            richTextBox_show_key.Clear();
        }

        private void button_ShowCoordinate_Click(object sender, EventArgs e)
        {
            if (button_ShowCoordinate.Text == "停止显示")
            {
                show_coordinate.Stop();
                button_ShowCoordinate.Text = "显示";
            }
            else if (button_ShowCoordinate.Text == "显示")
            {
                show_coordinate.Start();
                button_ShowCoordinate.Text = "停止显示";
            }
        }

        private void radioButton_light_on1_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on1.Checked == true)
            {
                Light_Ctrl(1, true);
            }
            else
            {
                Light_Ctrl(1, false);
            }
        }

        private void radioButton_light_on2_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on2.Checked == true)
            {
                Light_Ctrl(2, true);
            }
            else
            {
                Light_Ctrl(2, false);
            }
        }

        private void radioButton_light_on3_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on3.Checked == true)
            {
                Light_Ctrl(3, true);
            }
            else
            {
                Light_Ctrl(3, false);
            }
        }

        private void radioButton_light_on4_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on4.Checked == true)
            {
                Light_Ctrl(4, true);
            }
            else
            {
                Light_Ctrl(4, false);
            }
        }

        private void radioButton_light_on5_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on5.Checked == true)
            {
                Light_Ctrl(5, true);
            }
            else
            {
                Light_Ctrl(5, false);
            }
        }

        private void radioButton_light_on6_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on6.Checked == true)
            {
                Light_Ctrl(6, true);
            }
            else
            {
                Light_Ctrl(6, false);
            }
        }

        private void radioButton_light_on7_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on7.Checked == true)
            {
                Light_Ctrl(7, true);
            }
            else
            {
                Light_Ctrl(7, false);
            }
        }

        private void radioButton_light_on8_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_light_on8.Checked == true)
            {
                Light_Ctrl(8, true);
            }
            else
            {
                Light_Ctrl(8, false);
            }
        }

        private void radioButton_Vehicle_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_Vehicle.Checked)
            {
                barControlState = BarControlState.Vehicle;
                mainwindows_fv.receive_state = ReceiveState.VehiclePacket;
            }
        }

        private void radioButton_Servo_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_Servo.Checked)
            {
                barControlState = BarControlState.Servo;
                numericUpDown_ServoNum.Enabled = true;
                mainwindows_fv.receive_state = ReceiveState.OnlyServoPacket;
            }
        }

        private void button_BarControlStart_Click(object sender, EventArgs e)
        {
            if (button_BarControlStart.Text == "开始控制")
            {
                switch (barControlState)
                {
                    case BarControlState.Vehicle:
                        {
                            trackBar_Servo.Enabled = true;
                            trackBar_Motor.Enabled = true;

                            trackBar_Servo.Minimum = vehicleCommand_fv.SERVO_MIN;
                            trackBar_Servo.Maximum = vehicleCommand_fv.SERVO_MAX;
                            trackBar_Servo.Value = vehicleCommand_fv.SERVO_MID;
                            trackBar_Motor.Minimum = vehicleCommand_fv.SPEED_MIN;
                            trackBar_Motor.Maximum = vehicleCommand_fv.SPEED_MAX;
                            trackBar_Motor.Value = 0;
                            break;
                        }
                    case BarControlState.Servo:
                        {
                            trackBar_Motor.Enabled = false;
                            trackBar_Servo.Enabled = true;

                            trackBar_Servo.Value = vehicleCommand_fv.SERVO_MID;
                            if ((vehicleCommand_fv.SERVO_MID - 400) > 0)
                                trackBar_Servo.Minimum = vehicleCommand_fv.SERVO_MID - 400;
                            else
                                trackBar_Servo.Minimum = 0;

                            if ((vehicleCommand_fv.SERVO_MID + 400) < 1023)
                                trackBar_Servo.Maximum = vehicleCommand_fv.SERVO_MID + 400;
                            else
                                trackBar_Servo.Maximum = 1023;

                            trackBar_Motor.Value = 0;

                            Servo_Start(servo_num);
                            Delay(100);
                            Servo_Start(servo_num);
                            Delay(100);
                            Servo_Move_T(servo_num);
                            Delay(100);
                            byte svo_h = (byte)(vehicleCommand_fv.SERVO_MID >> 8);
                            byte svo_l = (byte)(vehicleCommand_fv.SERVO_MID & 0x00FF);
                            Servo_AddPoints_T(servo_num, svo_h, svo_l);
                            Delay(100);
                            button_ServoBreak.Enabled = true;
                            break;
                        }
                    default:
                        break;
                }
                BarControl.Start();
                barControl_start = true;
                radioButton_Servo.Enabled = false;
                radioButton_Vehicle.Enabled = false;
                numericUpDown_ServoNum.Enabled = false;
                button_BarControlReset.Enabled = true;
                button_BarControlStart.Text = "停止控制";
            }
            else
            {
                BarControl.Stop();
                trackBar_Servo.Enabled = false;
                trackBar_Motor.Enabled = false;
                barControl_start = false;
                radioButton_Servo.Enabled = true;
                radioButton_Vehicle.Enabled = true;
                button_BarControlReset.Enabled = false;
                button_ServoBreak.Enabled = false;
                if (radioButton_Servo.Checked)
                    numericUpDown_ServoNum.Enabled = true;
                button_BarControlStart.Text = "开始控制"; 
            }
        }

        private void button_BarControlReset_Click(object sender, EventArgs e)
        {
            switch (barControlState)
            {
                case BarControlState.Vehicle: //通过小车控制
                    {
                        trackBar_Servo.Minimum = vehicleCommand_fv.SERVO_MIN;
                        trackBar_Servo.Maximum = vehicleCommand_fv.SERVO_MAX;
                        trackBar_Servo.Value = vehicleCommand_fv.SERVO_MID;
                        trackBar_Motor.Minimum = vehicleCommand_fv.SPEED_MIN;
                        trackBar_Motor.Maximum = vehicleCommand_fv.SPEED_MAX;
                        trackBar_Motor.Value = 0;
                        break;
                    }
                case BarControlState.Servo: //直接控制舵机
                    {
                        trackBar_Servo.Value = vehicleCommand_fv.SERVO_MID;
                        if ((vehicleCommand_fv.SERVO_MID - 400) > 0)
                            trackBar_Servo.Minimum = vehicleCommand_fv.SERVO_MID - 400;
                        else
                            trackBar_Servo.Minimum = 0;

                        if ((vehicleCommand_fv.SERVO_MID + 400) < 1023)
                            trackBar_Servo.Maximum = vehicleCommand_fv.SERVO_MID + 400;
                        else
                            trackBar_Servo.Maximum = 1023;

                        trackBar_Motor.Value = 0;

                        break;
                    }
                default:
                    break;
            }
        }

        private void button_ServoBreak_Click(object sender, EventArgs e)
        {
            if (button_ServoBreak.Text == "紧急刹车")
            {
                Servo_Break(servo_num);
                button_ServoBreak.Text = "刹车恢复";
            }
            else
            {
                Servo_Start(servo_num);
                Servo_Move_T(servo_num);
                button_ServoBreak.Text = "紧急刹车";
            }
        }

        private void trackBar_Servo_ValueChanged(object sender, EventArgs e)
        {
            textBox_ShowServo.Text = trackBar_Servo.Value.ToString();
            servo_pos_bar = trackBar_Servo.Value;

            if (barControl_start && time_up)
            {
                time_up = false;
                switch (barControlState)
                {
                    case BarControlState.Vehicle: //通过小车控制
                        {
                            textBox_ShowServoDel.Text = (servo_pos_bar - vehicleCommand_fv.SERVO_MID).ToString();
                            byte svo_h, svo_l;
                            string svo_h_str, svo_l_str;
                            if (Math.Abs(servo_pos_bar - servo_pos_bar_temp) >= 10)
                            {
                                svo_h = (byte)(servo_pos_bar >> 8);
                                svo_l = (byte)(servo_pos_bar & 0x00FF);
                                if (svo_h >= 16)
                                {
                                    svo_h_str = Convert.ToString(svo_h, 16);
                                }
                                else
                                {
                                    svo_h_str = "0" + Convert.ToString(svo_h, 16);
                                }
                                if (svo_l >= 16)
                                {
                                    svo_l_str = Convert.ToString(svo_l, 16);
                                }
                                else
                                {
                                    svo_l_str = "0" + Convert.ToString(svo_l, 16);
                                }

                                Action_s(svo_h_str, svo_l_str);
                                servo_pos_bar_temp = servo_pos_bar;
                            }
                            break;
                        }
                    case BarControlState.Servo: //直接控制舵机
                        {
                            byte svo_h, svo_l;
                            if (Math.Abs(servo_pos_bar - servo_pos_bar_temp) >= 10)
                            {
                                svo_h = (byte)(servo_pos_bar >> 8);
                                svo_l = (byte)(servo_pos_bar & 0x00FF);

                                Servo_AddPoints_T(servo_num, svo_h, svo_l);
                                servo_pos_bar_temp = servo_pos_bar;
                            }

                            break;
                        }
                    default:
                        break;
                }
            }
        }

        private void trackBar_Motor_ValueChanged(object sender, EventArgs e)
        {
            textBox_ShowMotor.Text = trackBar_Motor.Value.ToString();
            int speed = trackBar_Motor.Value;

            if (barControl_start && time_up)
            {
                time_up = false;
                string dir_str, spd_h_str, spd_l_str;
                if (speed >= 0)
                {
                    dir_str = "00";
                }
                else
                {
                    dir_str = "01";
                    speed = -speed;
                }

                byte spd_h = (byte)(speed >> 8);
                byte spd_l = (byte)(speed & 0x0FF);

                if (spd_h >= 16)
                {
                    spd_h_str = Convert.ToString(spd_h, 16);
                }
                else
                {
                    spd_h_str = "0" + Convert.ToString(spd_h, 16);
                }

                if (spd_l >= 16)
                {
                    spd_l_str = Convert.ToString(spd_l, 16);
                }
                else
                {
                    spd_l_str = "0" + Convert.ToString(spd_l, 16);
                }

                Action_m(dir_str, spd_h_str, spd_l_str);
            }
        }

        private void numericUpDown_ServoNum_ValueChanged(object sender, EventArgs e)
        {
            servo_num = (byte)numericUpDown_ServoNum.Value;
        }
        #endregion

        #region 定时线程
        private void Ctest1_tick(object sender, ElapsedEventArgs e)
        {
            switch (Ctest1_state)
            {
                case 0:
                    Ctest1_count++;
                    if (Ctest1_times == 0)
                    {
                        Action("00", "03", "E8", "02", "26");    //速度+1000， 舵机+66
                        Ctest1_times = 1;
                    }
                    if (Ctest1_count >= 15)
                    {
                        Ctest1_state = 1;
                        Ctest1_count = 0;
                        Ctest1_times = 0;
                    }
                    break;

                case 1:
                    Ctest1_count++;
                    if (Ctest1_times == 0)
                    {
                        Action("00", "00", "00", "02", "26");    //速度0， 舵机+66
                        Ctest1_times = 1;
                    }
                    if (Ctest1_count >= 5)
                    {
                        Ctest1_state = 2;
                        Ctest1_count = 0;
                        Ctest1_times = 0;
                    }
                    break;

                case 2:
                    Ctest1_count++;
                    if (Ctest1_times == 0)
                    {
                        Action("01", "03", "E8", "02", "26");    //速度-1000， 舵机+66
                        Ctest1_times = 1;
                    }
                    if (Ctest1_count >= 15)
                    {
                        Ctest1_state = 3;
                        Ctest1_count = 0;
                        Ctest1_times = 0;
                    }
                    break;

                case 3:
                    Ctest1_count++;
                    if (Ctest1_times == 0)
                    {
                        Action("00", "00", "00", "01", "E4");    //速度0， 舵机0
                        Ctest1_times = 1;
                    }
                    if (Ctest1_count >= 5)
                    {
                        Ctest1_state = 4;
                        Ctest1_count = 0;
                        Ctest1_times = 0;
                    }
                    break;

                case 4:
                    Ctest1_count++;
                    if (Ctest1_times == 0)
                    {
                        Action("00", "03", "E8", "01", "A2");    //速度+1000， 舵机-66
                        Ctest1_times = 1;
                    }
                    if (Ctest1_count >= 15)
                    {
                        Ctest1_state = 5;
                        Ctest1_count = 0;
                        Ctest1_times = 0;
                    }
                    break;

                case 5:
                    Ctest1_count++;
                    if (Ctest1_times == 0)
                    {
                        Action("00", "00", "00", "01", "A2");    //速度0， 舵机0
                        Ctest1_times = 1;
                    }
                    if (Ctest1_count >= 5)
                    {
                        Ctest1_state = 6;
                        Ctest1_count = 0;
                        Ctest1_times = 0;
                    }
                    break;

                case 6:
                    Ctest1_count++;
                    if (Ctest1_times == 0)
                    {
                        Action("01", "03", "E8", "01", "A2");    //速度-1000， 舵机-66
                        Ctest1_times = 1;
                    }
                    if (Ctest1_count >= 15)
                    {
                        Ctest1_state = 100;
                        Ctest1_count = 0;
                        Ctest1_times = 0;
                    }
                    break;

                default:
                    if (Ctest1_times == 0)
                    {
                        Action("00", "00", "00", "01", "E4");    //速度0， 舵机0
                        Ctest1_times = 1;
                    }
                    break;
            }
        }

        private void Ctest2_tick(object sender, ElapsedEventArgs e)
        {
            
        }

        private void Ctest3_tick(object sender, ElapsedEventArgs e)
        {
            
        }

        private void Ctest4_tick(object sender, ElapsedEventArgs e)
        {

        }

        private void Key_Control_tick(object sender, ElapsedEventArgs e)
        {
            string dir_str_key = null;
            string spd_h_str_key = null;
            string spd_l_str_key = null;
            string svo_h_str_key = null;
            string svo_l_str_key = null;
            byte spd_h_key = 0;
            byte spd_l_key = 0;
            byte svo_h_key = 0;
            byte svo_l_key = 0;

            if (state_change == false)
            {
                #region 舵机偏转控制
                switch (key_state)
                {
                    case 0:
                        if(!flag_servo_hold)
                            servo_del_key = 0;
                        break;

                    case 1:   //直线前进
                        if (!flag_servo_hold)
                            servo_del_key = 0;

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("直线前进 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        break;

                    case 2:  //直线后退
                        if (!flag_servo_hold)
                            servo_del_key = 0;

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("直线后退 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        break;

                    case 3:  //左拐滑行
                        if (!flag_servo_hold)
                            servo_del_key += 50;
                        if (servo_del_key >= DEL_P)
                        {
                            servo_del_key = DEL_P;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("左拐滑行 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));


                        break;

                    case 4:  //右拐滑行
                        if (!flag_servo_hold)
                            servo_del_key -= 50;
                        if (servo_del_key <= DEL_N)
                        {
                            servo_del_key = DEL_N;
                        }
                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("右拐滑行 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        break;

                    case 5:  //左拐前进
                        if (!flag_servo_hold)
                            servo_del_key += 50;
                        if (servo_del_key >= DEL_P)
                        {
                            servo_del_key = DEL_P;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("左拐前进 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        break;

                    case 6:  //右拐前进
                        if (!flag_servo_hold)
                            servo_del_key -= 50;
                        if (servo_del_key <= DEL_N)
                        {
                            servo_del_key = DEL_N;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("右拐前进 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        break;

                    case 7:  //左拐后退
                        if (!flag_servo_hold)
                            servo_del_key += 50;
                        if (servo_del_key >= DEL_P)
                        {
                            servo_del_key = DEL_P;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("左拐后退 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        break;

                    case 8: //右拐后退
                        if (!flag_servo_hold)
                            servo_del_key -= 50;
                        if (servo_del_key <= DEL_N)
                        {
                            servo_del_key = DEL_N;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Green;
                            richTextBox_show_key.AppendText("右拐后退 \n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        break;

                    default:

                        break;
                }

                //将舵机信息转化为有效的数据包
                servo_key = (UInt16)(vehicleCommand_fv.SERVO_MID + servo_del_key);
                if ((Math.Abs(servo_temp_judge - servo_key) >= 10) || flag_sendcontinue)
                {
                    if (servo_key > vehicleCommand_fv.SERVO_MAX)
                    {
                        servo_key = (UInt16)vehicleCommand_fv.SERVO_MAX;
                    }
                    if (servo_key < vehicleCommand_fv.SERVO_MIN)
                    {
                        servo_key = (UInt16)vehicleCommand_fv.SERVO_MIN;
                    }
                    svo_h_key = (byte)(servo_key >> 8);
                    svo_l_key = (byte)(servo_key & 0x00FF);
                    if (svo_h_key >= 16)
                    {
                        svo_h_str_key = Convert.ToString(svo_h_key, 16);
                    }
                    else
                    {
                        svo_h_str_key = "0" + Convert.ToString(svo_h_key, 16);
                    }
                    if (svo_l_key >= 16)
                    {
                        svo_l_str_key = Convert.ToString(svo_l_key, 16);
                    }
                    else
                    {
                        svo_l_str_key = "0" + Convert.ToString(svo_l_key, 16);
                    }

                    Action_s(svo_h_str_key, svo_l_str_key);
                    servo_temp_judge = servo_key;
                }
                
                state_change = true;
                #endregion
            }
            else if (state_change == true)
            {
                #region 速度控制
                switch (speed_state)
                {
                    case 1:  //加速前进
                        if (flag_speed_change1 == false)
                        {
                            speed_key_ctrl = 0;
                            flag_speed_change1 = true;
                            flag_speed_change2 = false;
                        }
                        if (flag_speed_up_stop == false)
                        {
                            speed_down_count = 0;
                            speed_up_count++;
                            speed_key_ctrl += 400;
                        }

                        if (speed_key_ctrl >= vehicleCommand_fv.SPEED_MAX)
                        {
                            speed_key_ctrl = vehicleCommand_fv.SPEED_MAX;
                            flag_speed_up_stop = true;
                        }
                        else
                        {
                            flag_speed_up_stop = false;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Blue;
                            richTextBox_show_key.AppendText("加速前进 " + "  次数:" + speed_up_count.ToString() + "\n速度:" + speed_key_ctrl.ToString() + "\n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        
                        break;

                    case 2:  //加速后退
                        if (flag_speed_change2 == false)
                        {
                            speed_key_ctrl = 0;
                            flag_speed_change2 = true;
                            flag_speed_change1 = false;
                        }
                        if (flag_speed_up_stop_n == false)
                        {
                            speed_down_count = 0;
                            speed_up_count++;
                            speed_key_ctrl -= 400;
                        }

                        if (speed_key_ctrl <= vehicleCommand_fv.SPEED_MIN)
                        {
                            speed_key_ctrl = vehicleCommand_fv.SPEED_MIN;
                            flag_speed_up_stop_n = true;
                        }
                        else
                        {
                            flag_speed_up_stop_n = false;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Blue;
                            richTextBox_show_key.AppendText("加速后退 " + "  次数:" + speed_up_count.ToString() + "\n速度:" + speed_key_ctrl.ToString() + "\n");
                            richTextBox_show_key.ScrollToCaret();
                        }));


                        break;

                    case 3:  //减速前进
                        if (flag_speed_down_stop == false)
                        {
                            speed_up_count = 0;
                            speed_down_count++;
                            speed_key_ctrl -= 1200;
                        }

                        if (speed_key_ctrl <= 0)
                        {
                            speed_key_ctrl = 0;
                            flag_speed_down_stop = true;
                            speed_state = 0;
                        }
                        else
                        {
                            flag_speed_down_stop = false;
                        }
                        
                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Blue;
                            richTextBox_show_key.AppendText("减速前进 " + "  次数:" + speed_down_count.ToString() + "\n速度:" + speed_key_ctrl.ToString() + "\n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        
                        break;

                    case 4:  //减速后退
                        if (flag_speed_down_stop_n == false)
                        {
                            speed_up_count = 0;
                            speed_down_count++;
                            speed_key_ctrl += 1200;
                        }

                        if (speed_key_ctrl >= 0)
                        {
                            speed_key_ctrl = 0;
                            flag_speed_down_stop_n = true;
                            speed_state = 0;
                        }
                        else
                        {
                            flag_speed_down_stop_n = false;
                        }

                        richTextBox_show_key.Invoke(new EventHandler(delegate
                        {
                            richTextBox_show_key.SelectionColor = Color.Blue;
                            richTextBox_show_key.AppendText("减速后退 " + "  次数:" + speed_down_count.ToString() + "\n速度:" + speed_key_ctrl.ToString() + "\n");
                            richTextBox_show_key.ScrollToCaret();
                        }));

                        
                        break;

                    default:

                        break;
                }

                //将速度信息转化为有效的数据包格式
                if ((speed_key_ctrl != speed_temp_judge) || (flag_sendcontinue))
                {
                    if (speed_key_ctrl >= 0)
                    {
                        dir_str_key = "00";
                        speed_key = speed_key_ctrl;
                    }
                    else
                    {
                        dir_str_key = "01";
                        speed_key = -speed_key_ctrl;
                    }
                    spd_h_key = (byte)(speed_key >> 8);
                    spd_l_key = (byte)(speed_key & 0x0FF);

                    if (spd_h_key >= 16)
                    {
                        spd_h_str_key = Convert.ToString(spd_h_key, 16);
                    }
                    else
                    {
                        spd_h_str_key = "0" + Convert.ToString(spd_h_key, 16);
                    }

                    if (spd_l_key >= 16)
                    {
                        spd_l_str_key = Convert.ToString(spd_l_key, 16);
                    }
                    else
                    {
                        spd_l_str_key = "0" + Convert.ToString(spd_l_key, 16);
                    }

                    Action_m(dir_str_key, spd_h_str_key, spd_l_str_key);
                    speed_temp_judge = speed_key_ctrl;
                }

                state_change = false;

                #endregion
            }
        }

        private void Show_Coordinate_tick(object sender, ElapsedEventArgs e)
        {
            double x = 0;
            double y = 0;
            double s = 0;
            double agl = 0;

            x = mainwindows_fv.follower_x_coordinate;
            y = mainwindows_fv.follower_y_coordinate;
            s = mainwindows_fv.speed;
            agl = mainwindows_fv.angle;
            x = x / 1000;
            y = y / 1000;
            s = s / 1000;

            richTextBox_show_x.Invoke(new EventHandler(delegate
            {
                richTextBox_show_x.Clear();
            }));
            richTextBox_show_x.Invoke(new EventHandler(delegate
            {
                richTextBox_show_x.AppendText(x.ToString("g3"));
                richTextBox_show_x.SelectionColor = Color.Green;
                richTextBox_show_x.AppendText(" m");
            }));

            richTextBox_show_y.Invoke(new EventHandler(delegate
            {
                richTextBox_show_y.Clear();
            }));            
            richTextBox_show_y.Invoke(new EventHandler(delegate
            {
                richTextBox_show_y.AppendText(y.ToString("g3"));
                richTextBox_show_y.SelectionColor = Color.Green;
                richTextBox_show_y.AppendText(" m");
            }));

            richTextBox_show_speed.Invoke(new EventHandler(delegate
            {
                richTextBox_show_speed.Clear();
            }));
            richTextBox_show_speed.Invoke(new EventHandler(delegate
            {
                richTextBox_show_speed.AppendText(s.ToString("g3"));
                richTextBox_show_speed.SelectionColor = Color.Green;
                richTextBox_show_speed.AppendText(" m/s");
            }));

            richTextBox_show_angle.Invoke(new EventHandler(delegate
            {
                richTextBox_show_angle.Clear();
            }));
            richTextBox_show_speed.Invoke(new EventHandler(delegate
            {
                richTextBox_show_angle.AppendText(agl.ToString("g3"));
                richTextBox_show_angle.SelectionColor = Color.Green;
                richTextBox_show_angle.AppendText(" °");
            }));
        }

        private void BarControl_tick(object sender, ElapsedEventArgs e)
        {
            time_up = true;
        }
        #endregion   

    }
}
